module nade.physics.rigid_body;

import std.stdio;
import std.math;

import derelict.newton.newton;
import derelict.opengl.gl;

import nade.vector;
import nade.color;
import nade.matrix;

import nade.physics.world;
import nade.physics.collider;


class RigidBody : WorldObject {
	alias void delegate() DPhysics;
	alias void delegate() DDestructor;


	this(World world, Collider collider, const Color color = Color.LIGHT_GREY)
	{
		super(world);

		_collider = collider;
		_color = color;

		_phys = NewtonCreateBody(world.phys, collider.phys);
		NewtonBodySetTransformCallback(_phys, &cbSetTransform);
		NewtonBodySetDestructorCallback(_phys, &cbDestructor);
		NewtonBodySetUserData(_phys, cast(void*)this);

		world.add(this);
	}
	~this()
	{
		world.remove(this);

		if(get(_phys) !is null)
		{
			NewtonBodySetDestructorCallback(_phys, null);
			NewtonDestroyBody(world.phys, _phys);

			foreach(dg; _destructorCallbacks) dg();
		}

		delete _collider;
	}

	override void render()
	{
		glPushMatrix();
		glMultMatrixd(matrix.ptr);
		_color.render;
		_collider.render;
		glPopMatrix();
	}

	void enableGravity()
	{
		NewtonBodySetForceAndTorqueCallback(_phys, &cbApplyForceAndTorque);
	}
	void disableGravity()
	{
		NewtonBodySetForceAndTorqueCallback(_phys, null);
	}

	bool isFrozen()
	{
		return _frozen;
	}
	void freeze()
	{
		_frozen = true;
		NewtonBodySetAutoFreeze(_phys, 0);
		NewtonWorldFreezeBody(world.phys, _phys);
		velocity = Vector3.ZERO;
		NewtonBodySetMassMatrix(_phys, 0, 0, 0, 0);
	}
	void unfreeze()
	{
		_frozen = false;
		NewtonBodySetAutoFreeze(_phys, 1);
		NewtonWorldUnfreezeBody(world.phys, _phys);
		setMass(_mass);
	}
	void nudge()
	{
		_frozen = false;
		NewtonWorldUnfreezeBody(world.phys, _phys);
	}

	bool isLocked() { return _lock; }
	void lock() { _lock = true; }
	void unlock() { _lock = false; }

	void setMass(double mass)
	{
		_mass = mass;

		double ixx, iyy, izz;
		_collider.calculateInertia(_mass, ixx, iyy, izz);
		NewtonBodySetMassMatrix(_phys, _mass, ixx, iyy, izz);
	}


	Matrix44* matrix() { return &_matrix; }
	Matrix44* matrix(Matrix44 rhs)
	{
		_matrix = rhs;
		NewtonBodySetMatrix(_phys, _matrix.ptr);
		return &_matrix;
	}

	Vector3 position() { return _matrix.origin; }
	Vector3 position(Vector3 rhs)
	{
		_matrix.origin = rhs;
		NewtonBodySetMatrix(_phys, _matrix.ptr);
		return rhs;
	}
	Vector3 positionRecursive(Vector3 rhs)
	{
		_matrix.origin = rhs;
		NewtonBodySetMatrixRecursive(_phys, _matrix.ptr);
		return rhs;
	}

	Vector3 velocity()
	{
		double[3] velocity;
		NewtonBodyGetVelocity(_phys, velocity.ptr);
		return Vector3(velocity[0], velocity[1], velocity[2]);
	}
	Vector3 velocity(const Vector3 rhs)
	{
		NewtonBodySetVelocity(_phys, rhs.toArray!(double).ptr);
		return rhs.dup;
	}

	Vector3 angularVelocity()
	{
		double[3] omega;
		NewtonBodyGetOmega(_phys, omega.ptr);
		return Vector3(omega[0], omega[1], omega[2]);
	}
	Vector3 angularVelocity(const Vector3 rhs)
	{
		NewtonBodySetOmega(_phys, rhs.toArray!(double).ptr);
		return rhs.dup;
	}

	Vector3 angularDamping()
	{
		double[3] damping;
		NewtonBodyGetAngularDamping(_phys, damping.ptr);
		return Vector3(damping[0], damping[1], damping[2]);
	}
	Vector3 angularDamping(const Vector3 rhs)
	{
		NewtonBodySetAngularDamping(_phys, rhs.toArray!(double).ptr);
		return rhs.dup;
	}

	void applyImpulse(const Vector3 position, const Vector3 velocity)
	{
		NewtonAddBodyImpulse(_phys, velocity.toArray!(double).ptr, position.toArray!(double).ptr);
	}

	void addForce(const Vector3 force)
	{
		NewtonBodyAddForce(_phys, force.toArray!(double).ptr);
	}
	void addForce(const Vector3 position, const Vector3 force)
	{
		const offset = position - this.position;
		const torque = offset.cross(force);

		NewtonBodyAddForce(_phys, force.toArray!(double).ptr);
		NewtonBodyAddTorque(_phys, torque.toArray!(double).ptr);
	}
	void addForceLocal(const Vector3 force)
	{
		const gforce = _matrix.rotate(force);

		NewtonBodyAddForce(_phys, gforce.toArray!(double).ptr);
	}
	void addForceLocal(const Vector3 position, const Vector3 force)
	{
		const offset = _matrix.rotate(position);
		const gforce = _matrix.rotate(force);
		const torque = offset.cross(gforce);

		NewtonBodyAddForce(_phys, gforce.toArray!(double).ptr);
		NewtonBodyAddTorque(_phys, torque.toArray!(double).ptr);
	}

	void addPhysicsCallback(Object key, DPhysics dg) { _physicsCallbacks[key] = dg; }
	void removePhysicsCallback(Object key)
	{
		if((key in _physicsCallbacks) !is null)
			_physicsCallbacks.remove(key);
	}

	void addDestructorCallback(Object key, DDestructor dg) { _destructorCallbacks[key] = dg; }
	void removeDestructorCallback(Object key)
	{
		if((key in _destructorCallbacks) !is null)
			_destructorCallbacks.remove(key);
	}

	NewtonBody* phys() { return _phys; }

	Color color() { return _color; }
	Color color(const Color rhs) { return _color = rhs; }

	private {
		NewtonBody*		_phys;
		Collider		_collider;
		Color			_color;

		DPhysics[Object]		_physicsCallbacks;
		DDestructor[Object]		_destructorCallbacks;

		bool			_frozen = false;
		bool			_lock = false;
		double			_mass;
		Matrix44		_matrix;
	}

	static extern(C) void cbSetTransform(const NewtonBody* physBody, const double* matrix)
	{
		get(physBody)._matrix = matrix[0 .. 16];
	}

	static extern(C) void cbApplyForceAndTorque(const NewtonBody* physBody)
	{
		auto rigidBody = RigidBody.get(physBody);
		assert(rigidBody !is null);

		double[3] gravity = [0.0, -9.8, 0.0];
		// double[4] water = [0.0, 1.0, 0.0, 3];

		// auto sphere = cast(Collider.Sphere)RigidBody.get(physBody)._collider;
		// if(sphere !is null)
			// water[3] += 1 - fmax(fmax(sphere.radius.x, sphere.radius.y), sphere.radius.z);

		dFloat mass, ixx, iyy, izz;
		NewtonBodyGetMassMatrix(physBody, &mass, &ixx, &iyy, &izz);
		double[3] force = [mass*gravity[0], mass*gravity[1], mass*gravity[2]];

		NewtonBodySetForce(physBody, force.ptr);
		// NewtonBodyAddBuoyancyForce(physBody, 4, 0.9, 0.9, gravity.ptr, &cbWaterPlane, water.ptr);

		foreach(dg; rigidBody._physicsCallbacks) dg();
	}

	static extern(C) void cbWaterPlane(void *context, const double* globalSpaceMatrix, double* globalSpacePlane)
	{
		globalSpacePlane[0 .. 4] = (cast(double*)context)[0 .. 4];
	}

	static extern(C) void cbDestructor(const NewtonBody* physBody)
	{
		auto rigidBody = get(physBody);

		foreach(dg; rigidBody._destructorCallbacks) dg();

		NewtonBodySetDestructorCallback(physBody, null);
		NewtonBodySetUserData(physBody, null);

		delete rigidBody;
	}

	static RigidBody get(const NewtonBody* physBody)
	{
		return cast(RigidBody)NewtonBodyGetUserData(physBody);
	}
}
